/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "voslam_image.h"
#include "voslam_util.h"
#include "voslam_blas.h"
#include "voslamdef.h"
#include "voslam_geo.h"

/* gcc -O2 -Wall -g -c -o voslam_image.o voslam_image.c */

void detect_corners(BASETYPE *im,BASETYPE1 *quality,BASETYPE1 *w,int M,int N)
{
	int i,j;
	int Mi,Mi1,Mi2,Mi3,Mi4,i1;
	int Mnext[]={1,2,3,4,0},Mind;
	BASETYPE1 sA,sB,sC,fy,fx, *A, *B,*C;
	double m,n,d;
	

	A=w;
	B=A+5*M*sizeof(BASETYPE1);
	C=B+5*M*sizeof(BASETYPE1);
	for(i=0;i<5*M;i+=M)
	{
		for(j=0;j<M;j++)
		{
			A[i+j]=im[i+j]-im[i+M+M+j];
			B[i+j]=im[i+j]+2.*im[i+M+j]+im[i+M+M+j];
		}
	}
	for(i=0;i<5*M;i+=M)
	{
		for(j=0;j<M-2;j++)
		{
			fx=(A[i+j]+2.*A[i+j+1]+A[i+j+2])/8.;
			fy=(B[i+j]-B[i+j+2])/8.;
			A[i+j]=fx*fx;
			B[i+j]=fx*fy;
			C[i+j]=fy*fy;
		}
	}
	
	for(i=0;i<5*M;i+=M)
	{
		for(j=0;j<M-6;j++)
		{
			
			A[i+j]=A[i+j]+4.*A[i+j+1]+6.*A[i+j+2]+4.*A[i+j+3]+A[i+j+4];
			B[i+j]=B[i+j]+4.*B[i+j+1]+6.*B[i+j+2]+4.*B[i+j+3]+B[i+j+4];
			C[i+j]=C[i+j]+4.*C[i+j+1]+6.*C[i+j+2]+4.*C[i+j+3]+C[i+j+4];
		}
	}
	
	
	i=0;
	for(j=0;j<M-6;j++)
	{
		sA=(A[i+j]+4.*A[i+M+j]+6.*A[i+2*M+j]+4.*A[i+3*M+j]+A[i+4*M+j])/256.;
		sB=(B[i+j]+4.*B[i+M+j]+6.*B[i+2*M+j]+4.*B[i+3*M+j]+B[i+4*M+j])/256.;
		sC=(C[i+j]+4.*C[i+M+j]+6.*C[i+2*M+j]+4.*C[i+3*M+j]+C[i+4*M+j])/256.;
		/*det([A-s B;B C-s])
		(A-s)*(C-s)-B*B=A*C-BB-(A+C)*s+s*s*/
		m=(double)((sA+sC)*0.5);
		d=(double)(sA*sC-sB*sB);
		n = sqrt(m *m - d);
		quality[i+4+j]=m - n;
	}

 
	
	Mind=0;
	for(i=5*M;i<(N-3)*M;i+=M,Mind=Mnext[Mind])
	{
		Mi=M*Mind;
		for(j=0;j<M;j++)
		{
			A[Mi+j]=im[i+j]-im[i+M+M+j];
			B[Mi+j]=im[i+j]+2.*im[i+M+j]+im[i+M+M+j];
		}
	
		for(j=0;j<M-2;j++)
		{
			fx=(A[Mi+j]+2.*A[Mi+j+1]+A[Mi+j+2])/8.;
			fy=(B[Mi+j]-B[Mi+j+2])/8.;
			A[Mi+j]=fx*fx;
			B[Mi+j]=fx*fy;
			C[Mi+j]=fy*fy;
		}

		for(j=0;j<M-6;j++)
		{
			
			A[Mi+j]=A[Mi+j]+4.*A[Mi+j+1]+6.*A[Mi+j+2]+4.*A[Mi+j+3]+A[Mi+j+4];
			B[Mi+j]=B[Mi+j]+4.*B[Mi+j+1]+6.*B[Mi+j+2]+4.*B[Mi+j+3]+B[Mi+j+4];
			C[Mi+j]=C[Mi+j]+4.*C[Mi+j+1]+6.*C[Mi+j+2]+4.*C[Mi+j+3]+C[Mi+j+4];
		}
		i1=Mnext[Mind];
		Mi1=i1*M;
		i1=Mnext[i1];
		Mi2=i1*M;
		i1=Mnext[i1];
		Mi3=i1*M;
		i1=Mnext[i1];
		Mi4=i1*M;
		for(j=0;j<M-6;j++)
		{
			sA=(A[Mi1+j]+4.*A[Mi2+j]+6.*A[Mi3+j]+4.*A[Mi4+j]+A[Mi+j])/256.;
			sB=(B[Mi1+j]+4.*B[Mi2+j]+6.*B[Mi3+j]+4.*B[Mi4+j]+B[Mi+j])/256.;
			sC=(C[Mi1+j]+4.*C[Mi2+j]+6.*C[Mi3+j]+4.*C[Mi4+j]+C[Mi+j])/256.;
			/*det([A-s B;B C-s])
			(A-s)*(C-s)-B*B=A*C-BB-(A+C)*s+s*s*/
			m=(double)((sA+sC)*0.5);
			d=(double)(sA*sC-sB*sB);
			n = sqrt(m *m - d);
			quality[i+4+j]=m - n;

		}
	}

}

void detect_corners_tile(BASETYPE *im,BASETYPE1 *quality,BASETYPE1 *w,int M,int stride,int N)
{
    int i,j,ii;
    int Mi,Mi1,Mi2,Mi3,Mi4,i1;
    int Mnext[]={1,2,3,4,0},Mind;
    BASETYPE1 sA,sB,sC,fy,fx, *A, *B,*C;
    double m,n,d;

    A=w;
    B=A+5*M*sizeof(BASETYPE1);
    C=B+5*M*sizeof(BASETYPE1);
    for(i=0,ii=0;i<5*M;i+=M,ii+=stride)
    {
        for(j=0;j<M;j++)
        {
            A[i+j]=im[ii+j]-im[ii+stride+stride+j];
            B[i+j]=im[ii+j]+2.*im[ii+stride+j]+im[ii+stride+stride+j];
        }
    }
    for(i=0;i<5*M;i+=M)
    {
        for(j=0;j<M-2;j++)
        {
            fx=(A[i+j]+2.*A[i+j+1]+A[i+j+2])/8.;
            fy=(B[i+j]-B[i+j+2])/8.;
            A[i+j]=fx*fx;
            B[i+j]=fx*fy;
            C[i+j]=fy*fy;
        }
    }

    for(i=0;i<5*M;i+=M)
    {
        for(j=0;j<M-6;j++)
        {
            A[i+j]=A[i+j]+4.*A[i+j+1]+6.*A[i+j+2]+4.*A[i+j+3]+A[i+j+4];
            B[i+j]=B[i+j]+4.*B[i+j+1]+6.*B[i+j+2]+4.*B[i+j+3]+B[i+j+4];
            C[i+j]=C[i+j]+4.*C[i+j+1]+6.*C[i+j+2]+4.*C[i+j+3]+C[i+j+4];
        }
    }

    i=0;
    
    for(j=0;j<M-6;j++)
    {
        sA=(A[i+j]+4.*A[i+M+j]+6.*A[i+2*M+j]+4.*A[i+3*M+j]+A[i+4*M+j])/256.;
        sB=(B[i+j]+4.*B[i+M+j]+6.*B[i+2*M+j]+4.*B[i+3*M+j]+B[i+4*M+j])/256.;
        sC=(C[i+j]+4.*C[i+M+j]+6.*C[i+2*M+j]+4.*C[i+3*M+j]+C[i+4*M+j])/256.;
        m=(double)((sA+sC)*0.5);
        d=(double)(sA*sC-sB*sB);
        n = sqrt(m *m - d);
        quality[4+j]=m - n;
    }

    Mind=0;
    ii=5*stride;
    for(i=5*M;i<(N-3)*M;i+=M,ii+=stride,Mind=Mnext[Mind])
    {
        Mi=M*Mind;
        for(j=0;j<M;j++)
        {
            A[Mi+j]=im[ii+j]-im[ii+stride+stride+j];
            B[Mi+j]=im[ii+j]+2.*im[ii+stride+j]+im[ii+stride+stride+j];
        }

        for(j=0;j<M-2;j++)
        {
            fx=(A[Mi+j]+2.*A[Mi+j+1]+A[Mi+j+2])/8.;
            fy=(B[Mi+j]-B[Mi+j+2])/8.;
            A[Mi+j]=fx*fx;
            B[Mi+j]=fx*fy;
            C[Mi+j]=fy*fy;
        }

        for(j=0;j<M-6;j++)
        {
            A[Mi+j]=A[Mi+j]+4.*A[Mi+j+1]+6.*A[Mi+j+2]+4.*A[Mi+j+3]+A[Mi+j+4];
            B[Mi+j]=B[Mi+j]+4.*B[Mi+j+1]+6.*B[Mi+j+2]+4.*B[Mi+j+3]+B[Mi+j+4];
            C[Mi+j]=C[Mi+j]+4.*C[Mi+j+1]+6.*C[Mi+j+2]+4.*C[Mi+j+3]+C[Mi+j+4];
        }
        i1=Mnext[Mind];
        Mi1=i1*M;
        i1=Mnext[i1];
        Mi2=i1*M;
        i1=Mnext[i1];
        Mi3=i1*M;
        i1=Mnext[i1];
        Mi4=i1*M;
        for(j=0;j<M-6;j++)
        {
            sA=(A[Mi1+j]+4.*A[Mi2+j]+6.*A[Mi3+j]+4.*A[Mi4+j]+A[Mi+j])/256.;
            sB=(B[Mi1+j]+4.*B[Mi2+j]+6.*B[Mi3+j]+4.*B[Mi4+j]+B[Mi+j])/256.;
            sC=(C[Mi1+j]+4.*C[Mi2+j]+6.*C[Mi3+j]+4.*C[Mi4+j]+C[Mi+j])/256.;
            m=(double)((sA+sC)*0.5);
            d=(double)(sA*sC-sB*sB);
            n = sqrt(m *m - d);
            quality[ii+4+j]=m - n;
        }
    }
}

void find_loc_max2(BASETYPE1 *q,double *f,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *k1, int *k2, int M, int N, int maxNumOfFeatures)
{
    int i, j, k, iM,NM,K,index;
    BASETYPE1 maxq,val;
    
    k=0;
    NM=N*M;
    for(i=11;i<N-11;i++)
    {
        iM=i*M;
        for(j=11;j<M-11;j++)
        {
            if(q[iM+j]>Th)
            {
                if((q[iM+j]>q[iM+j+1])&&(q[iM+j]>q[iM+j-1])&&(q[iM+j]>q[iM+j+M])&&(q[iM+j]>q[iM+j-M]))
                {
                    if((q[iM+j]>q[iM+j+2])&&(q[iM+j]>q[iM+j-2])&&(q[iM+j]>q[iM+j+2*M])&&(q[iM+j]>q[iM+j-2*M]))
                    {
                        if((q[iM+j]>q[iM+j+1+M])&&(q[iM+j]>q[iM+j-1+M])&&(q[iM+j]>q[iM+j-M+1])&&(q[iM+j]>q[iM+j-M-1]))
                        {   
                            if((q[iM+j]>q[iM+j+1+2*M])&&(q[iM+j]>q[iM+j-1+2*M])&&(q[iM+j]>q[iM+j-2*M+1])&&(q[iM+j]>q[iM+j-2*M-1]))
                            {     
                                if((q[iM+j]>q[iM+j+2+M])&&(q[iM+j]>q[iM+j-2+M])&&(q[iM+j]>q[iM+j-M+2])&&(q[iM+j]>q[iM+j-M-2]))
                                {
                                    if((q[iM+j]>q[iM+j+2+2*M])&&(q[iM+j]>q[iM+j-2+2*M])&&(q[iM+j]>q[iM+j-2*M+2])&&(q[iM+j]>q[iM+j-2*M-2]))
                                    {
                                        tmp[k]=i;
                                        tmp[k+NM]=j;
                                        up_heap(ix,indx,-q[iM+j],k, &k);
                                        j++;
                                        j++;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    K=MIN(maxNumOfFeatures,k);
    if(k==0)
      k=1;
    down_heap(ix,indx,&maxq,&index,&k);
    f[0]=tmp[index];
    f[1]=tmp[index+NM];
    f[2]=-maxq;
    k2[0]=0;
    k1[0]=K;
    for(i=1;i<K;i++)
    {
        down_heap(ix,indx,&val,&index,&k);
        f[3*i]=tmp[index];
        f[3*i+1]=tmp[index+NM];
        f[3*i+2]=(int)-val;
        if(-maxq<5*-val)
            k2[0]++;
    }

}
void sp_find_loc_max2(BASETYPE1 *q,double *fea,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *k1, int *k2, int M, int N, int maxNumOfFeatures)
{
    int i, j, k, iM,NM,K,index,x,y;
    BASETYPE1 maxq,val;
    double dx,dy,a,b,c,d,e,f;
    
    k=0;
    NM=N*M;
    for(i=11;i<N-11;i++)
    {
        iM=i*M;
        for(j=11;j<M-11;j++)
        {
            if(q[iM+j]>Th)
            {
                if((q[iM+j]>q[iM+j+1])&&(q[iM+j]>q[iM+j-1])&&(q[iM+j]>q[iM+j+M])&&(q[iM+j]>q[iM+j-M]))
                {
                    if((q[iM+j]>q[iM+j+2])&&(q[iM+j]>q[iM+j-2])&&(q[iM+j]>q[iM+j+2*M])&&(q[iM+j]>q[iM+j-2*M]))
                    {
                        if((q[iM+j]>q[iM+j+1+M])&&(q[iM+j]>q[iM+j-1+M])&&(q[iM+j]>q[iM+j-M+1])&&(q[iM+j]>q[iM+j-M-1]))
                        {   
                            if((q[iM+j]>q[iM+j+1+2*M])&&(q[iM+j]>q[iM+j-1+2*M])&&(q[iM+j]>q[iM+j-2*M+1])&&(q[iM+j]>q[iM+j-2*M-1]))
                            {     
                                if((q[iM+j]>q[iM+j+2+M])&&(q[iM+j]>q[iM+j-2+M])&&(q[iM+j]>q[iM+j-M+2])&&(q[iM+j]>q[iM+j-M-2]))
                                {
                                    if((q[iM+j]>q[iM+j+2+2*M])&&(q[iM+j]>q[iM+j-2+2*M])&&(q[iM+j]>q[iM+j-2*M+2])&&(q[iM+j]>q[iM+j-2*M-2]))
                                    {
                                        tmp[k]=i;
                                        tmp[k+NM]=j;
                                        up_heap(ix,indx,-q[iM+j],k, &k);
                                        j++;
                                        j++;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    K=MIN(maxNumOfFeatures,k);
    if(k==0)
      k=1;
    down_heap(ix,indx,&maxq,&index,&k);
    x=tmp[index];
    y=tmp[index+NM];
    a=(q[(x-1)*M+y-1]+q[(x-1)*M+y]+q[(x-1)*M+y+1]-2*q[x*M+y-1]-2*q[x*M+y]-2*q[x*M+y+1]+q[(x+1)*M+y-1]+q[(x+1)*M+y]+q[(x+1)*M+y+1])/3.;
    b=(q[(x-1)*M+y-1]-q[(x-1)*M+y+1]-q[(x+1)*M+y-1]+q[(x+1)*M+y+1])/4.;
    c=(q[(x-1)*M+y-1]-2*q[(x-1)*M+y]+q[(x-1)*M+y+1]+q[x*M+y-1]-2*q[x*M+y]+q[x*M+y+1]+q[(x+1)*M+y-1]-2*q[(x+1)*M+y]+q[(x+1)*M+y+1])/3.;
    d=(-q[(x-1)*M+y-1]-q[(x-1)*M+y]-q[(x-1)*M+y+1]+q[(x+1)*M+y-1]+q[(x+1)*M+y]+q[(x+1)*M+y+1])/6.;
    e=(-q[(x-1)*M+y-1]+q[(x-1)*M+y+1]-q[x*M+y-1]+q[x*M+y+1]-q[(x+1)*M+y-1]+q[(x+1)*M+y+1])/6.;
    f=(-q[(x-1)*M+y-1]+2*q[(x-1)*M+y]-q[(x-1)*M+y+1]+2*q[x*M+y-1]+5*q[x*M+y]+2*q[x*M+y+1]-q[(x+1)*M+y-1]+2*q[(x+1)*M+y]-q[(x+1)*M+y+1])/9.;

    dy=(a*e-b*d)/(b*b-a*c);
    dx=(c*d-b*e)/(b*b-a*c);

    if(fabs(dx)>0.5)
    {
        dx=0;
        dy=0;
    }
   
    if(fabs(dy)>0.5)
    {
        dx=0;
        dy=0;
    }
    fea[0]=tmp[index]+dx;
    fea[1]=tmp[index+NM]+dy;
    fea[2]=-maxq;
    k2[0]=0;
    k1[0]=K;
    for(i=1;i<K;i++)
    {
        down_heap(ix,indx,&val,&index,&k);
        x=tmp[index];
        y=tmp[index+NM];
        a=(q[(x-1)*M+y-1]+q[(x-1)*M+y]+q[(x-1)*M+y+1]-2*q[x*M+y-1]-2*q[x*M+y]-2*q[x*M+y+1]+q[(x+1)*M+y-1]+q[(x+1)*M+y]+q[(x+1)*M+y+1])/3.;
        b=(q[(x-1)*M+y-1]-q[(x-1)*M+y+1]-q[(x+1)*M+y-1]+q[(x+1)*M+y+1])/4.;
        c=(q[(x-1)*M+y-1]-2*q[(x-1)*M+y]+q[(x-1)*M+y+1]+q[x*M+y-1]-2*q[x*M+y]+q[x*M+y+1]+q[(x+1)*M+y-1]-2*q[(x+1)*M+y]+q[(x+1)*M+y+1])/3.;
        d=(-q[(x-1)*M+y-1]-q[(x-1)*M+y]-q[(x-1)*M+y+1]+q[(x+1)*M+y-1]+q[(x+1)*M+y]+q[(x+1)*M+y+1])/6.;
        e=(-q[(x-1)*M+y-1]+q[(x-1)*M+y+1]-q[x*M+y-1]+q[x*M+y+1]-q[(x+1)*M+y-1]+q[(x+1)*M+y+1])/6.;
        f=(-q[(x-1)*M+y-1]+2*q[(x-1)*M+y]-q[(x-1)*M+y+1]+2*q[x*M+y-1]+5*q[x*M+y]+2*q[x*M+y+1]-q[(x+1)*M+y-1]+2*q[(x+1)*M+y]-q[(x+1)*M+y+1])/9.;

        dy=(a*e-b*d)/(b*b-a*c);
        dx=(c*d-b*e)/(b*b-a*c);

        if(fabs(dx)>0.5)
        {
            dx=0;
            dy=0;
        }
   
        if(fabs(dy)>0.5)
        {
            dx=0;
            dy=0;
        }
   
        fea[3*i]=tmp[index]+dx;
        fea[3*i+1]=tmp[index+NM]+dy;
        fea[3*i+2]=(int)-val;
        if(-maxq<5*-val)
            k2[0]++;
    }

}
void sp_find_loc_max_4tile(BASETYPE1 *q,double *fea,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *num_fea, int *k2, int M, int N, int max_num_of_features)
{
  int k1,k0;
  
  sp_find_loc_max_tile(q,fea,Th,tmp,ix,indx,num_fea,k2,M/2+11,N/2+11,0,0,M,max_num_of_features/4);
  sp_find_loc_max_tile(q+M/2-11,fea+3*num_fea[0],Th,tmp,ix,indx,&k1,&k0,M/2+11,N/2+11,M/2-11,0,M,max_num_of_features/4);
  num_fea[0]+=k1;
  k2[0]+=k0;
  sp_find_loc_max_tile(q+(N/2-11)*M,fea+3*num_fea[0],Th,tmp,ix,indx,&k1,&k0,M/2+11,N/2+11,0,N/2-11,M,max_num_of_features/4);
  num_fea[0]+=k1;
  k2[0]+=k0;
  sp_find_loc_max_tile(q+(N/2-11)*M+M/2-11,fea+3*num_fea[0],Th,tmp,ix,indx,&k1,&k0,M/2+11,N/2+11,M/2-11,N/2-11,M,max_num_of_features/4);
  num_fea[0]+=k1;
  k2[0]+=k0;
}
void sp_find_loc_max_tile(BASETYPE1 *q,double *fea,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *k1, int *k2, int M, int N, int oM, int oN, int stride, int maxNumOfFeatures)
{
    int i, j, k, iM,NM,K,index,x,y;
    BASETYPE1 maxq,val;
    double dx,dy,a,b,c,d,e,f;
    
    k=0;
    NM=N*M;
    for(i=11;i<N-11;i++)
    {
        iM=i*stride;
        for(j=11;j<M-11;j++)
        {
            if(q[iM+j]>Th)
            {
                if((q[iM+j]>q[iM+j+1])&&(q[iM+j]>q[iM+j-1])&&(q[iM+j]>q[iM+j+stride])&&(q[iM+j]>q[iM+j-stride]))
                {
                    if((q[iM+j]>q[iM+j+2])&&(q[iM+j]>q[iM+j-2])&&(q[iM+j]>q[iM+j+2*stride])&&(q[iM+j]>q[iM+j-2*stride]))
                    {
                        if((q[iM+j]>q[iM+j+1+stride])&&(q[iM+j]>q[iM+j-1+stride])&&(q[iM+j]>q[iM+j-stride+1])&&(q[iM+j]>q[iM+j-stride-1]))
                        {   
                            if((q[iM+j]>q[iM+j+1+2*stride])&&(q[iM+j]>q[iM+j-1+2*stride])&&(q[iM+j]>q[iM+j-2*stride+1])&&(q[iM+j]>q[iM+j-2*stride-1]))
                            {     
                                if((q[iM+j]>q[iM+j+2+stride])&&(q[iM+j]>q[iM+j-2+stride])&&(q[iM+j]>q[iM+j-M+2])&&(q[iM+j]>q[iM+j-stride-2]))
                                {
                                    if((q[iM+j]>q[iM+j+2+2*stride])&&(q[iM+j]>q[iM+j-2+2*stride])&&(q[iM+j]>q[iM+j-2*stride+2])&&(q[iM+j]>q[iM+j-2*stride-2]))
                                    {
                                        tmp[k]=i;
                                        tmp[k+NM]=j;
                                        up_heap(ix,indx,-q[iM+j],k, &k);
                                        j++;
                                        j++;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    
    K=MIN(maxNumOfFeatures,k);
    if(k==0)
      k=1;
    down_heap(ix,indx,&maxq,&index,&k);
    x=tmp[index];
    y=tmp[index+NM];
    a=(q[(x-1)*stride+y-1]+q[(x-1)*stride+y]+q[(x-1)*stride+y+1]-2*q[x*stride+y-1]-2*q[x*stride+y]-2*q[x*stride+y+1]+q[(x+1)*stride+y-1]+q[(x+1)*stride+y]+q[(x+1)*stride+y+1])/3.;
    b=(q[(x-1)*stride+y-1]-q[(x-1)*stride+y+1]-q[(x+1)*stride+y-1]+q[(x+1)*stride+y+1])/4.;
    c=(q[(x-1)*stride+y-1]-2*q[(x-1)*stride+y]+q[(x-1)*stride+y+1]+q[x*stride+y-1]-2*q[x*stride+y]+q[x*stride+y+1]+q[(x+1)*stride+y-1]-2*q[(x+1)*stride+y]+q[(x+1)*stride+y+1])/3.;
    d=(-q[(x-1)*stride+y-1]-q[(x-1)*stride+y]-q[(x-1)*stride+y+1]+q[(x+1)*stride+y-1]+q[(x+1)*stride+y]+q[(x+1)*stride+y+1])/6.;
    e=(-q[(x-1)*stride+y-1]+q[(x-1)*stride+y+1]-q[x*stride+y-1]+q[x*stride+y+1]-q[(x+1)*stride+y-1]+q[(x+1)*stride+y+1])/6.;
    f=(-q[(x-1)*stride+y-1]+2*q[(x-1)*stride+y]-q[(x-1)*stride+y+1]+2*q[x*stride+y-1]+5*q[x*stride+y]+2*q[x*stride+y+1]-q[(x+1)*stride+y-1]+2*q[(x+1)*stride+y]-q[(x+1)*stride+y+1])/9.;

    dy=(a*e-b*d)/(b*b-a*c);
    dx=(c*d-b*e)/(b*b-a*c);

    if(fabs(dx)>0.5)
    {
        dx=0;
        dy=0;
    }
   
    if(fabs(dy)>0.5)
    {
        dx=0;
        dy=0;
    }
    fea[0]=tmp[index]+dx;
    fea[1]=tmp[index+NM]+dy;
    fea[2]=-maxq;
    k2[0]=0;
    k1[0]=K;
    for(i=1;i<K;i++)
    {
        down_heap(ix,indx,&val,&index,&k);
        x=tmp[index];
        y=tmp[index+NM];
        a=(q[(x-1)*stride+y-1]+q[(x-1)*stride+y]+q[(x-1)*stride+y+1]-2*q[x*stride+y-1]-2*q[x*stride+y]-2*q[x*stride+y+1]+q[(x+1)*stride+y-1]+q[(x+1)*stride+y]+q[(x+1)*stride+y+1])/3.;
        b=(q[(x-1)*stride+y-1]-q[(x-1)*stride+y+1]-q[(x+1)*stride+y-1]+q[(x+1)*stride+y+1])/4.;
        c=(q[(x-1)*stride+y-1]-2*q[(x-1)*stride+y]+q[(x-1)*stride+y+1]+q[x*stride+y-1]-2*q[x*stride+y]+q[x*stride+y+1]+q[(x+1)*stride+y-1]-2*q[(x+1)*stride+y]+q[(x+1)*stride+y+1])/3.;
        d=(-q[(x-1)*stride+y-1]-q[(x-1)*stride+y]-q[(x-1)*stride+y+1]+q[(x+1)*stride+y-1]+q[(x+1)*stride+y]+q[(x+1)*stride+y+1])/6.;
        e=(-q[(x-1)*stride+y-1]+q[(x-1)*stride+y+1]-q[x*stride+y-1]+q[x*stride+y+1]-q[(x+1)*stride+y-1]+q[(x+1)*stride+y+1])/6.;
        f=(-q[(x-1)*stride+y-1]+2*q[(x-1)*stride+y]-q[(x-1)*stride+y+1]+2*q[x*stride+y-1]+5*q[x*stride+y]+2*q[x*stride+y+1]-q[(x+1)*stride+y-1]+2*q[(x+1)*stride+y]-q[(x+1)*stride+y+1])/9.;

        dy=(a*e-b*d)/(b*b-a*c);
        dx=(c*d-b*e)/(b*b-a*c);

        if(fabs(dx)>0.5)
        {
            dx=0;
            dy=0;
        }
   
        if(fabs(dy)>0.5)
        {
            dx=0;
            dy=0;
        }
   
        fea[3*i]=tmp[index]+dx+oN;
        fea[3*i+1]=tmp[index+NM]+dy+oM;
        fea[3*i+2]=(int)-val;
        if(-maxq<5*-val)
            k2[0]++;
    }

}



double match_features1(BASETYPE *im1, BASETYPE *im2, int N)
{
    int i;
    double res,res1,res2,res3,res4;
    
    res=0;
    for(i=0;i<N-4;i+=4)
    {
        res1=fabs(im1[i]-im2[i]);            
        res2=fabs(im1[i+1]-im2[i+1]);            
        res3=fabs(im1[i+2]-im2[i+2]);            
        res4=fabs(im1[i+3]-im2[i+3]);            
        res+=res1+res2+res3+res4;
    }
    res+=fabs(im1[N-1]-im2[N-1]);            

    return(res);
}
BASETYPE2 match_features21(BASETYPE2 *im1, BASETYPE2 *im2, int N)
{
    int i;
    BASETYPE2 res,res1,res2,res3,res4,var1,var2;
    
    res=(BASETYPE2)0;
    var1=0;
    var2=0;
    for(i=0;i<N-4;i+=4)
    {

        res1=im1[i]*im2[i];            
        res2=im1[i+1]*im2[i+1];            
        res3=im1[i+2]*im2[i+2];            
        res4=im1[i+3]*im2[i+3];            
        res+=res1+res2+res3+res4;
        res1=im1[i]*im1[i];            
        res2=im1[i+1]*im1[i+1];            
        res3=im1[i+2]*im1[i+2];            
        res4=im1[i+3]*im1[i+3]; 
        var1+=res1+res2+res3+res4;
        res1=im2[i]*im2[i];            
        res2=im2[i+1]*im2[i+1];            
        res3=im2[i+2]*im2[i+2];            
        res4=im2[i+3]*im2[i+3]; 
        var2+=res1+res2+res3+res4;
    }
        for(;i<N;i++)
    {
          res+=im1[i]*im2[i];            
          var1+=im1[i]*im1[i];
          var2+=im2[i]*im2[i];
    }
    res=res/sqrt(var1*var2);
    
    return(res);
}
BASETYPE2 match_features2(BASETYPE2 *im1, BASETYPE2 *im2, int N)
{
    int i;
    BASETYPE2 res,res1,res2,res3,res4;
    
    res=(BASETYPE2)0;

    for(i=0;i<N-4;i+=4)
    {

        res1=im1[i]*im2[i];            
        res2=im1[i+1]*im2[i+1];            
        res3=im1[i+2]*im2[i+2];            
        res4=im1[i+3]*im2[i+3];            
        res+=res1+res2+res3+res4;

    }
    for(;i<N;i++)
    {
          res+=im1[i]*im2[i];            
    }
    
    return(res);
}

void match_corners(double *res,double *strength, int  *matches, BASETYPE *patchesA, BASETYPE *patchesB,int *tmp, double *featuresA,double *featuresB,int numFeaturesA, int numFeaturesB,int wxy,int th)
{
    int i,j,index,x1,x2,y1,y2;
    int minval;


    for(i=0;i<numFeaturesA;i++)
    {
        x1=featuresA[3*i];
        y1=featuresA[3*i+1];
        x2=featuresB[0];
        y2=featuresB[1];
        res[i*numFeaturesB]=6000000.;
        if((abs(x1-x2)<th)&&(abs(y1-y2)<th))
        {  
            res[i*numFeaturesB]=match_features1(patchesA+i*wxy,patchesB,wxy);
        }
        minval=res[i*numFeaturesB];
        index=0;
        for(j=1;j<numFeaturesB;j++)
        {
            x2=featuresB[3*j];
            y2=featuresB[3*j+1];
            res[i*numFeaturesB+j]=6000000.;
            if((abs(x1-x2)<th)&&(abs(y1-y2)<th))
            {
            
                res[i*numFeaturesB+j]=match_features1(patchesA+i*wxy,patchesB+j*wxy,wxy);
            }
            if(res[i*numFeaturesB+j]<minval)
            {
                minval=res[i*numFeaturesB+j];
                index=j;
            }
        }
        tmp[i]=index;
        matches[i]=0;
        strength[i]=6000000.;
    }	
  

  
    for(i=0;i<numFeaturesB;i++)
    {
        minval=res[i];
        index=0;
        for(j=1;j<numFeaturesA;j++)
        {
            if(res[j*numFeaturesB+i]<minval)
            {
                minval=res[j*numFeaturesB+i];
                index=j;
            }
        }
        if(tmp[index]==i)
        {
            if(minval<6000000.)
            {
                matches[index]=i+1;
                strength[index]=minval;
            }
        }
    }
}

void match_cornerscf(BASETYPE2 *reso, int  *matcho,BASETYPE2 *resl, int  *matchl, BASETYPE2 *patchesA, BASETYPE2 *patchesB, double *featuresA,double *featuresB,int num_featuresA, int num_featuresB,int wxy, int th)
{
  
    int i,j,index,x1,x2,y1,y2;
    BASETYPE2 res;
    
    for(j=0;j<num_featuresA;j++)
    {
        matcho[j]=0;
        reso[j]=(BASETYPE2)0;
    }
    for(i=0;i<num_featuresB;i++)
    {
        x1=featuresB[3*i];
        y1=featuresB[3*i+1];
        x2=featuresA[0];
        y2=featuresA[1];

        if(abs(x1-x2)<th&&abs(y1-y2)<th)
        {  
            res=match_features2(patchesB+i*wxy,patchesA,wxy);
            matchl[i]=1;
            resl[i]=res;
            if(res>reso[0])
            {
                matcho[0]=i+1;
                reso[0]=res;
            }
        }
        
        for(j=1;j<num_featuresA;j++)
        {
      
            x2=featuresA[3*j];
            y2=featuresA[3*j+1];

            if(abs(x1-x2)<th&&abs(y1-y2)<th)
            {
                res=match_features2(patchesB+i*wxy,patchesA+j*wxy,wxy);
                if(res>resl[i])
                {
                    matchl[i]=j+1;
                    resl[i]=res;
                }
                if(res>reso[j])
                {
                    matcho[j]=i+1;
                    reso[j]=res;
                }
            }
        }
    }
    j=0;
    for(i=0;i<num_featuresA;i++)
    {
        if(matcho[i]>0)
        {
            index=matcho[i]-1;
            if(matchl[index]!=i+1)
            {
                matcho[i]=0; 
            }
        }
    }
}

void match_cornerscE(double *E,BASETYPE2 *reso, int  *matcho,BASETYPE2 *resl, int  *matchl, BASETYPE2 *patchesA, BASETYPE2 *patchesB, double *featuresA,double *featuresB,int num_featuresA, int num_featuresB,int wxy, int th)
{
  
    int i,j,index;
    double num,den,a1,b1,c1,ee,res,x1,x2,y1,y2;
    
    for(j=0;j<num_featuresA;j++)
    {
        matcho[j]=0;
        reso[j]=0;
    }
    for(j=0;j<num_featuresB;j++)
    {
        matchl[j]=0;
        resl[j]=-2;
    }
    for(i=0;i<num_featuresB;i++)
    {
        x1=featuresB[3*i];
        y1=featuresB[3*i+1];
        x2=featuresA[0];
        y2=featuresA[1];
        a1=E[0]*x1+E[3]*y1+E[6];    
        b1=E[1]*x1+E[4]*y1+E[7];
        c1=E[2]*x1+E[5]*y1+E[8];
        num=sqrt((a1*a1+b1*b1));
        den=a1*x2+b1*y2+c1;
        ee=den/num;
        if(ee<th)
        {
            if(ee>-th)
            {
                res=match_features2(patchesB+i*wxy,patchesA,wxy);
                matchl[i]=1;
                resl[i]=res;
                if(res>reso[0])
                {
                    matcho[0]=i+1;
                    reso[0]=res;
                }
            }
        }
        
        for(j=1;j<num_featuresA;j++)
        {
      
            x2=featuresA[3*j];
            y2=featuresA[3*j+1];

            den=a1*x2+b1*y2+c1;
            ee=den*num;
            if(ee<th)
            {
                if(ee>-th)
                {
                    res=match_features2(patchesB+i*wxy,patchesA+j*wxy,wxy);
                    if(res>resl[i])
                    {
                        matchl[i]=j+1;
                        resl[i]=res;
                    }
                    if(res>reso[j])
                    {
                        matcho[j]=i+1;
                        reso[j]=res;
                    }
                }
            }
        }
    }

    for(i=0;i<num_featuresA;i++)
    {
        if(matcho[i]>0)
        {
            index=matcho[i]-1;
            if(matchl[index]!=i+1)
            {
                matcho[i]=0; 
            }
        }
    }
}
void match_cornerc(BASETYPE2 *strength, int  *matches, BASETYPE2 *patchA, BASETYPE2 *patchesB, double *featureA,double *featuresB, int numFeaturesB,int wxy, int th)
{
    int j,index,x1,x2,y1,y2;
    BASETYPE2 maxval,maxval2,res;
    
    x1=featureA[0];
    y1=featureA[1];
    x2=featuresB[0];
    y2=featuresB[1];
    res=(BASETYPE2)-1.;
    index=-1;
    if(abs(x1-x2)<th&&abs(y1-y2)<th)
    {  
        res=match_features2(patchA,patchesB,wxy);
        index=0;
    }
    maxval=res;
    maxval2=-1;
    
    for(j=1;j<numFeaturesB;j++)
    {  
        x2=featuresB[3*j];
        y2=featuresB[3*j+1];
        res=(BASETYPE2)-1.;
        if(abs(x1-x2)<th&&abs(y1-y2)<th)
        {
            
            res=match_features2(patchA,patchesB+j*wxy,wxy);
            if(res>maxval)
            {
                maxval2=maxval;
                maxval=res;
                index=j;
            }
            else if(res>maxval2)
            {
                maxval2=res;
            }
        }   
    }   
   matches[0]=index+1;
   strength[0]=maxval;
   strength[1]=maxval2;

}

void match_cornersc(BASETYPE2 *res,BASETYPE2 *strength, int  *matches, BASETYPE2 *patchesA, BASETYPE2 *patchesB,int *tmp, double *featuresA,double *featuresB,int numFeaturesA, int numFeaturesB,int wxy, int th)
{
  
    int i,j,index,x1,x2,y1,y2;
    BASETYPE2 maxval,maxval2;
    

    for(i=0;i<numFeaturesA;i++)
    {
        x1=featuresA[3*i];
        y1=featuresA[3*i+1];
        x2=featuresB[0];
        y2=featuresB[1];
        res[i*numFeaturesB]=(BASETYPE2)-1.;
        if(abs(x1-x2)<th&&abs(y1-y2)<th)
        {  
            res[i*numFeaturesB]=match_features2(patchesA+i*wxy,patchesB,wxy);
        }
        maxval=res[i*numFeaturesB];
        
        index=0;
        for(j=1;j<numFeaturesB;j++)
        {
      
            x2=featuresB[3*j];
            y2=featuresB[3*j+1];
            res[i*numFeaturesB+j]=(BASETYPE2)-1.;
            if(abs(x1-x2)<th&&abs(y1-y2)<th)
            {
            
                res[i*numFeaturesB+j]=match_features2(patchesA+i*wxy,patchesB+j*wxy,wxy);
            }
            if(res[i*numFeaturesB+j]>maxval)
            {
                maxval=res[i*numFeaturesB+j];
                index=j;
            }
            
            
        }   
        tmp[i]=index;
        matches[i]=0;
        strength[2*i]=(BASETYPE2)-1;
        strength[2*i+1]=(BASETYPE2)-1;
    }   
  

  
    for(i=0;i<numFeaturesB;i++)
    {
        maxval=res[i];
        maxval2=0;
        index=0;
        for(j=1;j<numFeaturesA;j++)
        {
            if(res[j*numFeaturesB+i]>maxval)
            {
                maxval2=maxval;
                maxval=res[j*numFeaturesB+i];
                index=j;
            }
            else if(res[j*numFeaturesB+i]>maxval2)
            {
                maxval2=res[j*numFeaturesB+i];
            }
              
        }
        if(tmp[index]==i)
        {
            matches[index]=i+1;
            strength[2*index]=maxval;
            strength[2*index+1]=maxval2;
        }
    }
}
void match_cornerscl(BASETYPE2 *res,BASETYPE2 *strength, int  *matches, BASETYPE2 *patchesA, BASETYPE2 *patchesB,int *tmp, double *featuresA,double *featuresB,int numFeaturesA, int numFeaturesB,int wxy, int th, float th1, float th2)
{
  
    int i,j,index,x1,x2,y1,y2;
    BASETYPE2 maxval,maxval2;
    

    for(i=0;i<numFeaturesA;i++)
    {
        x1=featuresA[3*i];
        y1=featuresA[3*i+1];
        x2=featuresB[0];
        y2=featuresB[1];
        res[i*numFeaturesB]=(BASETYPE2)-1.;
        if(abs(x1-x2)<th&&abs(y1-y2)<th)
        {  
            res[i*numFeaturesB]=match_features2(patchesA+i*wxy,patchesB,wxy);
        }
        maxval=res[i*numFeaturesB];
        index=0;
        for(j=1;j<numFeaturesB;j++)
        {
      
            x2=featuresB[3*j];
            y2=featuresB[3*j+1];
            res[i*numFeaturesB+j]=(BASETYPE2)-1.;
            if(abs(x1-x2)<th&&abs(y1-y2)<th)
            {
            
                res[i*numFeaturesB+j]=match_features2(patchesA+i*wxy,patchesB+j*wxy,wxy);
            }
            if(res[i*numFeaturesB+j]>maxval)
            {
                maxval=res[i*numFeaturesB+j];
                index=j;
            }
        }   
        tmp[i]=index;
        matches[i]=0;
        strength[2*i]=(BASETYPE2)-1;
        strength[2*i+1]=(BASETYPE2)-1;
    }   
  

  
    for(i=0;i<numFeaturesB;i++)
    {
        maxval=res[i];
        maxval2=0.;
        index=0;
        for(j=1;j<numFeaturesA;j++)
        {
            if(res[j*numFeaturesB+i]>maxval)
            {
                maxval2=maxval;
                maxval=res[j*numFeaturesB+i];
                index=j;
            }
            else if(res[j*numFeaturesB+i]>maxval2)
            {
                maxval2=res[j*numFeaturesB+i];
            } 
        }
        if(tmp[index]==i)
        {
            if(maxval>th1)
            {
                if(th2*maxval>maxval2)
                {
                    matches[index]=i+1;
                    strength[2*index]=maxval;
                    strength[2*index+1]=maxval2;
                }
            }
        }
    }
}

void extract_patches(BASETYPE *patches,BASETYPE *im,double *features,int numFeatures,int wx,int wy, int M)
{
    int x_start,y_start,x_stop,y_stop;
    int i,j,k,m,x,y,wxy;
 
    x_start=-wx;
    x_stop=wx;
    y_start=-wy;
    y_stop=wy;
    wxy=(2*wx+1)*(2*wy+1);

    for(i=0;i<numFeatures;i++)
    {
        x=features[3*i];
        y=features[3*i+1];
        m=0;
        for(k=x_start;k<=x_stop;k++)
        {
            for(j=y_start;j<=y_stop;j++)
            {
                patches[i*wxy+m]=im[(k+x)*M+y+j];
                m++;
            }
        }
    }
}
void extract_patchesn(BASETYPE2 *patches, BASETYPE *im, double *points,int numpoints,int wx,int wy, int M)
{
    double norm;
    int x_start,y_start,x_stop,y_stop;
    int i,j,k,l,m,x,y,wxy,mean;
 
    x_start=-wx;
    x_stop=wx;
    y_start=-wy;
    y_stop=wy;
    wxy=(2*wx+1)*(2*wy+1);
   
    for(i=0;i<numpoints;i++)
    {
        x=points[3*i];
        y=points[3*i+1];
        l=0;
        mean=0;
        for(k=x_start;k<=x_stop;k++)
        {
            for(j=y_start;j<=y_stop;j++)
            {
                mean+=im[(k+x)*M+y+j];
                l++;
            }
        }
        mean=mean/l;
        m=0;
        norm=0;
        for(k=x_start;k<=x_stop;k++)
        {
            for(j=y_start;j<=y_stop;j++)
            {
                patches[i*wxy+m]=im[(k+x)*M+y+j]-mean;
                norm+=patches[i*wxy+m]*patches[i*wxy+m];
                m++;
            }   
        }
        norm=sqrt(norm);
        for(m=0;m<wxy;m++)
            patches[i*wxy+m]=patches[i*wxy+m]/norm;
    }
}

void sp_extract_patches(BASETYPE *patches,BASETYPE *im,double *points,int num_points,int wx,int wy,int M)
{
    int i,j,k,xo,yo,m,wxy;
    double x,y,ax,ay,w1,w2,w3,w4;
    
    wxy=(2*wx+1)*(2*wy+1);
    
    for(k=0;k<num_points;k++)
    {

        m=0;
        for(i=-wx;i<=wx;i++)
        {
            for(j=-wy;j<=wy;j++)
            {
                x=points[3*k]+i;
                y=points[3*k+1]+j;

                xo=(int)(x);
                yo=(int)(y);
                ax=(x)-xo;
                ay=(y)-yo;

                w1=(1-ay)*(1-ax);
                w2=(1-ay)*ax;
                w3=ay*(1-ax);
                w4=ay*ax;
                patches[k*wxy+m]=(w1*im[(xo)*M+yo]+w2*im[(xo+1)*M+yo]+w3*im[(xo)*M+yo+1]+w4*im[(xo+1)*M+yo+1]);
                m++;
            }
        }
    }
}
void sp_extract_patchesn(BASETYPE2 *patches,BASETYPE *im,double *points,int num_points,int wx,int wy,int M)
{
    int i,j,k,xo,yo,m,wxy;
    double x,y,ax,ay,w1,w2,w3,w4,mean,norm;
    
    wxy=(2*wx+1)*(2*wy+1);
    
    for(k=0;k<num_points;k++)
    {

        m=0;
        mean=0;
        for(i=-wx;i<=wx;i++)
        {
            for(j=-wy;j<=wy;j++)
            {
                x=points[3*k]+i;
                y=points[3*k+1]+j;

                xo=(int)(x);
                yo=(int)(y);
                ax=(x)-xo;
                ay=(y)-yo;

                w1=(1-ay)*(1-ax);
                w2=(1-ay)*ax;
                w3=ay*(1-ax);
                w4=ay*ax;
                patches[k*wxy+m]=w1*im[(xo)*M+yo]+w2*im[(xo+1)*M+yo]+w3*im[(xo)*M+yo+1]+w4*im[(xo+1)*M+yo+1];
                mean+=patches[k*wxy+m];
                m++;
            }
        }
        mean=mean/m;
        norm=0;
        for(i=0;i<m;i++)
        {
                patches[k*wxy+i]=patches[k*wxy+i]-mean;
                norm+=patches[k*wxy+i]*patches[k*wxy+i];
               
        }
        norm=sqrt(norm);
        for(i=0;i<m;i++)
            patches[k*wxy+i]=patches[k*wxy+i]/norm;
    }
}

void undist_points31(double *warped, double *pts, double *p, int cyc, int N)
{
    
    double x,y,r2,r4,r6,u,v;
    double u0,v0,f,k1,k2,k3;
    int i,j;

    f=p[0];
    u0=p[1];
    v0=p[2];
    k1=p[3];
    k2=p[4];
    k3=p[5];
    
    for(i=0;i<N;i++)
    {
        u=(pts[3*i]-u0)/f;
        v=(pts[3*i+1]-v0)/f;
        x=u;
        y=v;
        for(j=0;j<cyc;j++)
        {
            r2=x*x+y*y;
            r4=r2*r2;
            r6=r4*r2;
            x=u/(1+k1*r2+k2*r4+k3*r6);
            y=v/(1+k1*r2+k2*r4+k3*r6);
        }
        warped[3*i]=f*x+u0;
        warped[3*i+1]=f*y+v0;
    }
}
void undist_points3(double *warped, double *pts, double *p, int cyc, int N)
{
	int i,j;
	double u0,v0,f,k1,k2,k3;
	double x,y,x1,y1,xm,ym,ssd,ssdmin,r2,r4,r6,u,v,JJ[4],Je[2],J[4],e[2],g[2],L[4],lambda;
   
        lambda=0.1;
        f=p[0];
        u0=p[1];
        v0=p[2];
        k1=p[3];
        k2=p[4];
        k3=p[5];
	for(i=0;i<N;i++)
	{
     		
		u=(pts[3*i]-u0)/f;
		v=(pts[3*i+1]-v0)/f;
		xm=u;
		ym=v;
		x=xm;
		y=ym;
		r2=x*x+y*y;
	        r4=r2*r2;
		r6=r4*r2;
		x1=x*(1+k1*r2+k2*r4+k3*r6);
                e[0]=u-x1;
		y1=y*(1+k1*r2+k2*r4+k3*r6);
                e[1]=v-y1;
		ssdmin=e[0]*e[0]+e[1]*e[1];	
		for(j=0;j<cyc;j++)
		{
		        J[0]=1+k1*(3*x*x+y*y)+k2*(5*x*x*x*x+6*x*x*y*y+y*y*y*y)+k3*(7*x*x*x*x*x*x+15*x*x*x*x*y*y+9*x*x*y*y*y*y+y*y*y*y*y*y);
                        J[1]=k1*(2*x*y)+k2*(4*y*y*y*x+4*y*x*x*x)+k3*(6*y*y*y*y*y*x+12*y*y*y*x*x*x+6*y*x*x*x*x*x);
                           J[3]=1+k1*(3*y*y+x*x)+k2*(5*y*y*y*y+6*y*y*x*x+x*x*x*x)+k3*(7*y*y*y*y*y*y+15*y*y*y*y*x*x+9*y*y*x*x*x*x+x*x*x*x*x*x);
                        J[2]=J[1];
			JJ[0]=J[0]*J[0]+J[2]*J[2]+lambda;
			JJ[1]=J[0]*J[1]+J[2]*J[3];
			JJ[2]=J[1]*J[0]+J[3]*J[2];
			JJ[3]=J[1]*J[1]+J[3]*J[3]+lambda;
			Je[0]=J[0]*e[0]+J[2]*e[1];
			Je[1]=J[1]*e[0]+J[3]*e[1];
			choldc(L,JJ,2);
                        cholsl(L,Je,g,2);
                        x=x+g[0];
			y=y+g[1];
			r2=x*x+y*y;
			r4=r2*r2;
			r6=r4*r2;
			x1=x*(1+k1*r2+k2*r4+k3*r6);
                        e[0]=u-x1;
                        y1=y*(1+k1*r2+k2*r4+k3*r6);
                        e[1]=v-y1;
	//		printf("er: %f %f \n",e[0],e[1]);
			ssd=e[0]*e[0]+e[1]*e[1];
			if(ssd<=ssdmin)
			{
			    ssdmin=ssd;
			    xm=x;
			    ym=y;
			    lambda=lambda/7;
			}
			else
			{
			    lambda=lambda*9;
			    x=xm;
			    y=ym;
			    r2=x*x+y*y;
			    r4=r2*r2;
			    r6=r4*r2;
			    x1=x*(1+k1*r2+k2*r4+k3*r6);
                            e[0]=u-x1;
                            y1=y*(1+k1*r2+k2*r4+k3*r6);
                            e[1]=v-y1;
			}
                        
        	}
		warped[3*i]=f*x+u0;
		warped[3*i+1]=f*y+v0;
	}
}

void dist_points3(double *warped, double *pts, double *p, int N)
{
    
    double x,y,r2,r4,r6;
    double u0,v0,f,k1,k2,k3;
    int i;

    f=p[0];
    u0=p[1];
    v0=p[2];
    k1=p[3];
    k2=p[4];
    k3=p[5];
    
    for(i=0;i<N;i++)
    {
        x=(pts[2*i]-u0)/f;
        y=(pts[2*i+1]-v0)/f;
       
        r2=x*x+y*y;
        r4=r2*r2;
        r6=r4*r2;
	
        warped[2*i]=p[1]+p[0]*(1+p[3]*r2+p[4]*r4+p[5]*r6)*x;
        warped[2*i+1]=p[2]+p[0]*(1+p[3]*r2+p[4]*r4+p[5]*r6)*y;
    }
}
void normalise_points(double *fA,double *pts,double *p,int N)
{
    int i;
    
    for(i=0;i<N;i++)
    {
        fA[2*i]=(pts[2*i]-p[1])/p[0];
        fA[2*i+1]=(pts[2*i+1]-p[2])/p[0];
    }
}
void project_landmarks(
                double *v,
                double *p,
                double *pts,
                double *X,
                int *maski,
                int *count,              
                int num_landmarks,
                int M,
                int N
                )
{
    double P[12],z,x,y,pts1[2];
    int i=0,index,cnt;
    
    quater_2_rot(P,v);

    P[9]=v[4];
    P[10]=v[5];
    P[11]=v[6];

    cnt=0;
    
    for(i=0;i<num_landmarks;i++)
    {
        index=maski[i];
        if(index==0)
        { 
             /* projecting object 3D-coordinates to camera frame */
            x=P[0]*X[3*i]+P[3]*X[1+3*i]+P[6]*X[2+3*i]+P[9];
            y=P[1]*X[3*i]+P[4]*X[1+3*i]+P[7]*X[2+3*i]+P[10];
            z=P[2]*X[3*i]+P[5]*X[1+3*i]+P[8]*X[2+3*i]+P[11];

            if(z>0)
            {
                pts[cnt*2]=p[1]+p[0]*x/z;
                pts[1+cnt*2]=p[2]+p[0]*y/z;
                dist_points3(pts1, pts+cnt*2, p,1);
                if(pts1[0]>0&&pts1[0]<M&&pts1[1]>0&&pts1[1]<N)
                    cnt++;
            }
        }
    }
    (*count)=cnt;
}
void project_landmarks1(
                double *v,
                double *p,
                double *pts,
                double *X,
                int *maski,
                int *count,              
                int num_landmarks,
                int M,
                int N
                )
{
    double P[12],z,x,y;
    int i=0,index,cnt;
    
    quater_2_rot(P,v);

    P[9]=v[4];
    P[10]=v[5];
    P[11]=v[6];

    cnt=0;
    
    for(i=0;i<num_landmarks;i++)
    {
        index=maski[i];
        if(index==0)
        { 
             /* projecting object 3D-coordinates to camera frame */
            x=P[0]*X[3*i]+P[3]*X[1+3*i]+P[6]*X[2+3*i]+P[9];
            y=P[1]*X[3*i]+P[4]*X[1+3*i]+P[7]*X[2+3*i]+P[10];
            z=P[2]*X[3*i]+P[5]*X[1+3*i]+P[8]*X[2+3*i]+P[11];

            if(z>0)
            {
                pts[cnt*2]=p[1]+p[0]*x/z;
                pts[1+cnt*2]=p[2]+p[0]*y/z;
        
                if(pts[cnt*2]>0&&pts[cnt*2]<M&&pts[1+cnt*2]>0&&pts[1+cnt*2]<N)
                    cnt++;
            }
        }
    }
    (*count)=cnt;
}
void find_nearest_point_to_landmark(
                double *v,
                double *p,
                double *fea,
                double *X,
                double *mindist,
                int *ind,        
                int num_points,
                int M,
                int N
                )
{
    double P[12],z,x,y,ppts[2],dist;
    int i=0,D=7;
    
    quater_2_rot(P,v);

    P[9]=v[4];
    P[10]=v[5];
    P[11]=v[6];

    x=P[0]*X[0]+P[3]*X[1]+P[6]*X[2]+P[9];
    y=P[1]*X[0]+P[4]*X[1]+P[7]*X[2]+P[10];
    z=P[2]*X[0]+P[5]*X[1]+P[8]*X[2]+P[11];
    *mindist=1000.;
    
    *ind=0;
    if(z>0)
    {
        ppts[0]=p[1]+p[0]*x/z;
        ppts[1]=p[2]+p[0]*y/z;
        
        if(ppts[0]<0||ppts[0]>M||ppts[1]<0||ppts[1]>N)
            return;
    }
    else
      return;
    
    for(i=0;i<num_points;i++)
    {
        if(fea[i*D+3]<0)
        {
            dist=sqrt((ppts[0]-fea[D*i])*(ppts[0]-fea[D*i])+(ppts[1]-fea[D*i+1])*(ppts[1]-fea[D*i+1]));
            if(dist<mindist[0])
            {
                (*mindist)=dist;
                (*ind)=i;
            }
        }
    }

}

void slam_find_nearest_point_to_landmark(
                double *v,
                double *p,
                double *fea,
                double *X,
                double *mindist,
                int *ind,        
                int num_points,
                int M,
                int N
                )
{
    double P[12],z,x,y,ppts[2],dist;
    int i=0,D=8;
    
    quater_2_rot(P,v);

    P[9]=v[4];
    P[10]=v[5];
    P[11]=v[6];

    x=P[0]*X[0]+P[3]*X[1]+P[6]*X[2]+P[9];
    y=P[1]*X[0]+P[4]*X[1]+P[7]*X[2]+P[10];
    z=P[2]*X[0]+P[5]*X[1]+P[8]*X[2]+P[11];
    *mindist=1000.;
    
    *ind=0;
    if(z>0)
    {
        ppts[0]=p[1]+p[0]*x/z;
        ppts[1]=p[2]+p[0]*y/z;
        
        if(ppts[0]<0||ppts[0]>M||ppts[1]<0||ppts[1]>N)
            return;
    }
    else
      return;
    
    for(i=0;i<num_points;i++)
    {
        if(fea[i*D+3]<0)
        {
            dist=sqrt((ppts[0]-fea[D*i])*(ppts[0]-fea[D*i])+(ppts[1]-fea[D*i+1])*(ppts[1]-fea[D*i+1]));
            if(dist<mindist[0])
            {
                (*mindist)=dist;
                (*ind)=i;
            }
        }
    }

}



void warp_image(BASETYPE *warped,BASETYPE *im,double *u,double *v,double *iu,double *iv,double s,int M,int N)
{
    int i,j,k,l,dk,dl,xo,yo;
    double ia,pa,e1,e2,p;
   

    
    for(i=0;i<N-1;i++)
    {
        for(j=0;j<M-1;j++)
        {
            xo=(int)u[i*M+j];
            yo=(int)v[i*M+j];
            
	    /*
	    the corresponding point for (i,j) in undistorted image is point (xo,yo) in the distorted image 
	    */
            
            pa=0;
            ia=0;
            k=xo-2;
            if(k<0)
                k=0;
            dk=xo+2;
            if(dk>=N)
                dk=N-1;
            
            dl=yo+2;
            if(dl>=M)
                dl=M-1;
    
            for(;k<=dk;k++)
            {
                l=yo-2;
                if(l<0)
                    l=0;
                for(;l<=dl;l++)
                {
                    /* distance is calculated in the warped coordinates of points */
                    e1=iu[k*M+l]-i;
                    e2=iv[k*M+l]-j;
                    p=exp(-s*(e1*e1+e2*e2));
                    
                    pa+=p;
                    ia+=p*im[k*M+l];
                }
            }
            warped[i*M+j]=ia/pa;    
        }
    }
}
void warp_image_fast(BASETYPE *warped,BASETYPE *im,double *u,double *v,int M,int N)
{
    int i,j,yo,xo;
    double ax,ay;
   

    
    for(i=0;i<N;i++)
    {
        for(j=0;j<M;j++)
        {
            
            xo=(int)u[i*M+j];
            yo=(int)v[i*M+j];
            if((xo+1>=N)||((xo)<0)||((yo)<0)||((yo+1)>=M))
            {
                warped[i*M+j]=0;
            }
            else
            {
                ax=u[i*M+j]-xo;
                ay=v[i*M+j]-yo;
                warped[i*M+j]=(1-ay)*(1-ax)*im[(xo)*M+yo]+ax*(1-ay)*im[(xo+1)*M+yo]+(1-ax)*ay*im[(xo)*M+yo+1]+ax*ay*im[(xo+1)*M+yo+1];
            }
        }
    }
}

void pyr_down(BASETYPE *im, BASETYPE *pim,int M, int N)
{
    int i,j;

    for(i=1;i<N-1;i++)
    {
        for(j=1;j<M-1;j++)
        {
           
           pim[i*M+j]=(im[4*i*M-4*M+2*j-2]+ 4*im[4*i*M-2*M+2*j-2]+ 6*im[4*i*M+2*j-2]+ 4*im[4*i*M+2*M+2*j-2]+  im[4*i*M+4*M+2*j-2]
                    +4*im[4*i*M-4*M+2*j-1]+16*im[4*i*M-2*M+2*j-1]+24*im[4*i*M+2*j-1]+16*im[4*i*M+2*M+2*j-1]+4*im[4*i*M+4*M+2*j-1]
                    +6*im[4*i*M-4*M+2*j]  +24*im[4*i*M-2*M+2*j]  +36*im[4*i*M+2*j]  +24*im[4*i*M+2*M+2*j]  +6*im[4*i*M+4*M+2*j]
                    +4*im[4*i*M-4*M+2*j+1]+16*im[4*i*M-2*M+2*j+1]+24*im[4*i*M+2*j+1]+16*im[4*i*M+2*M+2*j+1]+4*im[4*i*M+4*M+2*j+1]
                    +  im[4*i*M-4*M+2*j+2]+ 4*im[4*i*M-2*M+2*j+2]+ 6*im[4*i*M+2*j+2]+ 4*im[4*i*M+2*M+2*j+2]+  im[4*i*M+4*M+2*j+2])/256;
        }
        pim[i*M]=(     im[4*i*M-4*M]  + 4*im[4*i*M-2*M]  + 6*im[4*i*M]  + 4*im[4*i*M+2*M]  +  im[4*i*M+4*M]
                    +4*im[4*i*M-4*M]  +16*im[4*i*M-2*M]  +24*im[4*i*M]  +16*im[4*i*M+2*M]  +4*im[4*i*M+4*M]
                    +6*im[4*i*M-4*M]  +24*im[4*i*M-2*M]  +36*im[4*i*M]  +24*im[4*i*M+2*M]  +6*im[4*i*M+4*M]
                    +4*im[4*i*M-4*M+1]+16*im[4*i*M-2*M+1]+24*im[4*i*M+1]+16*im[4*i*M+2*M+1]+4*im[4*i*M+4*M+1]
                    +  im[4*i*M-4*M+2]+ 4*im[4*i*M-2*M+2]+ 6*im[4*i*M+2]+ 4*im[4*i*M+2*M+2]+  im[4*i*M+4*M+2])/256;
        j=M-1;            
        pim[i*M+j]=(im[4*i*M-4*M+2*j-2]+ 4*im[4*i*M-2*M+2*j-2]+ 6*im[4*i*M+2*j-2]+ 4*im[4*i*M+2*M+2*j-2]+  im[4*i*M+4*M+2*j-2]
                    +4*im[4*i*M-4*M+2*j-1]+16*im[4*i*M-2*M+2*j-1]+24*im[4*i*M+2*j-1]+16*im[4*i*M+2*M+2*j-1]+4*im[4*i*M+4*M+2*j-1]
                    +6*im[4*i*M-4*M+2*j]  +24*im[4*i*M-2*M+2*j]  +36*im[4*i*M+2*j]  +24*im[4*i*M+2*M+2*j]  +6*im[4*i*M+4*M+2*j]
                    +4*im[4*i*M-4*M+2*j+1]+16*im[4*i*M-2*M+2*j+1]+24*im[4*i*M+2*j+1]+16*im[4*i*M+2*M+2*j+1]+4*im[4*i*M+4*M+2*j+1]
                    +  im[4*i*M-4*M+2*j+1]+ 4*im[4*i*M-2*M+2*j+1]+ 6*im[4*i*M+2*j+1]+ 4*im[4*i*M+2*M+2*j+1]+  im[4*i*M+4*M+2*j+1])/256;
    
    }
    i=N-1;
    for(j=1;j<M-1;j++)
    {
         pim[j]=(      im[2*j-2]+ 4*im[2*j-2]+ 6*im[2*j-2]+ 4*im[2*M+2*j-2]+  im[4*M+2*j-2]
                    +4*im[2*j-1]+16*im[2*j-1]+24*im[2*j-1]+16*im[2*M+2*j-1]+4*im[4*M+2*j-1]
                    +6*im[2*j]  +24*im[2*j]  +36*im[2*j]  +24*im[2*M+2*j]  +6*im[4*M+2*j]
                    +4*im[2*j+1]+16*im[2*j+1]+24*im[2*j+1]+16*im[2*M+2*j+1]+4*im[4*M+2*j+1]
                    +  im[2*j+2]+ 4*im[2*j+2]+ 6*im[2*j+2]+ 4*im[2*M+2*j+2]+  im[4*M+2*j+2])/256;
        
                    
         pim[i*M+j]=(  im[4*i*M-4*M+2*j-2]+ 4*im[4*i*M-2*M+2*j-2]+ 6*im[4*i*M+2*j-2]+ 4*im[4*i*M+2*M+2*j-2]+  im[4*i*M+2*M+2*j-2]
                    +4*im[4*i*M-4*M+2*j-1]+16*im[4*i*M-2*M+2*j-1]+24*im[4*i*M+2*j-1]+16*im[4*i*M+2*M+2*j-1]+4*im[4*i*M+2*M+2*j-1]
                    +6*im[4*i*M-4*M+2*j]  +24*im[4*i*M-2*M+2*j]  +36*im[4*i*M+2*j]  +24*im[4*i*M+2*M+2*j]  +6*im[4*i*M+2*M+2*j]
                    +4*im[4*i*M-4*M+2*j+1]+16*im[4*i*M-2*M+2*j+1]+24*im[4*i*M+2*j+1]+16*im[4*i*M+2*M+2*j+1]+4*im[4*i*M+2*M+2*j+1]
                    +  im[4*i*M-4*M+2*j+2]+ 4*im[4*i*M-2*M+2*j+2]+ 6*im[4*i*M+2*j+2]+ 4*im[4*i*M+2*M+2*j+2]+  im[4*i*M+2*M+2*j+2])/256;
    }
 
    pim[0]=  (         im[0]+ 4*im[0]+ 6*im[0]+ 4*im[2*M]  +  im[4*M]
                    +4*im[0]+16*im[0]+24*im[0]+16*im[2*M]  +4*im[4*M]
                    +6*im[0]+24*im[0]+36*im[0]+24*im[2*M]  +6*im[4*M]
                    +4*im[1]+16*im[1]+24*im[1]+16*im[2*M+1]+4*im[4*M+1]
                    +  im[2]+ 4*im[2]+ 6*im[2]+ 4*im[2*M+2]+  im[4*M+2])/256;
     
    i=N-1;
    j=M-1; 
    pim[i*M+j]=(       im[4*i*M-4*M+2*j-2]+ 4*im[4*i*M-2*M+2*j-2]+ 6*im[4*i*M+2*j-2]+ 4*im[4*i*M+2*M+2*j-2]+  im[4*i*M+2*M+2*j-2]
                    +4*im[4*i*M-4*M+2*j-1]+16*im[4*i*M-2*M+2*j-1]+24*im[4*i*M+2*j-1]+16*im[4*i*M+2*M+2*j-1]+4*im[4*i*M+2*M+2*j-1]
                    +6*im[4*i*M-4*M+2*j]  +24*im[4*i*M-2*M+2*j]  +36*im[4*i*M+2*j]  +24*im[4*i*M+2*M+2*j]  +6*im[4*i*M+2*M+2*j]
                    +4*im[4*i*M-4*M+2*j+1]+16*im[4*i*M-2*M+2*j+1]+24*im[4*i*M+2*j+1]+16*im[4*i*M+2*M+2*j+1]+4*im[4*i*M+2*M+2*j+1]
                    +  im[4*i*M-4*M+2*j+1]+ 4*im[4*i*M-2*M+2*j+1]+ 6*im[4*i*M+2*j+1]+ 4*im[4*i*M+2*M+2*j+1]+  im[4*i*M+2*M+2*j+1])/256;

    pim[j]=(           im[2*j-2]+ 4*im[2*j-2]+ 6*im[2*j-2]+ 4*im[2*M+2*j-2]+  im[4*M+2*j-2]
                    +4*im[2*j-1]+16*im[2*j-1]+24*im[2*j-1]+16*im[2*M+2*j-1]+4*im[4*M+2*j-1]
                    +6*im[2*j]  +24*im[2*j]  +36*im[2*j]  +24*im[2*M+2*j]  +6*im[4*M+2*j]
                    +4*im[2*j+1]+16*im[2*j+1]+24*im[2*j+1]+16*im[2*M+2*j+1]+4*im[4*M+2*j+1]
                    +  im[2*j+1]+ 4*im[2*j+1]+ 6*im[2*j+1]+ 4*im[2*M+2*j+1]+  im[4*M+2*j+1])/256;
    
    pim[i*M]=(         im[4*i*M-4*M]  + 4*im[4*i*M-2*M]  + 6*im[4*i*M]  + 4*im[4*i*M+2*M]  +  im[4*i*M+2*M]
                    +4*im[4*i*M-4*M]  +16*im[4*i*M-2*M]  +24*im[4*i*M]  +16*im[4*i*M+2*M]  +4*im[4*i*M+2*M]
                    +6*im[4*i*M-4*M]  +24*im[4*i*M-2*M]  +36*im[4*i*M]  +24*im[4*i*M+2*M]  +6*im[4*i*M+2*M]
                    +4*im[4*i*M-4*M+1]+16*im[4*i*M-2*M+1]+24*im[4*i*M+1]+16*im[4*i*M+2*M+1]+4*im[4*i*M+2*M+1]
                    +  im[4*i*M-4*M+2]+ 4*im[4*i*M-2*M+2]+ 6*im[4*i*M+2]+ 4*im[4*i*M+2*M+2]+  im[4*i*M+2*M+2])/256;
}
void orient3D_patch(double *q,double *t,double *n,double *Xc, double dist)
{
    double om[3],theta,norm,R[9],d,nn[3],X[3];
    
/*calculate rotation */
    norm=sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);    
    theta=acos(n[2]/norm);
    q[0]=cos(theta/2);
    om[0]=n[1];
    om[1]=-n[0];
    om[2]=0;
    norm=sqrt(om[0]*om[0]+om[1]*om[1]);
    if(norm>2*1e-16)
    {
        q[1]=sin(theta/2)*om[0]/(norm);
        q[2]=sin(theta/2)*om[1]/(norm);
        q[3]=0;
    }
    else
    {
        q[0]=1.;
        q[1]=0.;
        q[2]=0.;
        q[3]=0.;
    }
    quater_2_rot(R,q);
        
    norm=sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
    nn[0]=(R[0]*n[0]+R[3]*n[1]+R[6]*n[2])/norm;
    nn[1]=(R[1]*n[0]+R[4]*n[1]+R[7]*n[2])/norm;
    nn[2]=(R[2]*n[0]+R[5]*n[1]+R[8]*n[2])/norm;

/*calculate translation */

    
    d=1./(n[0]*Xc[0]/Xc[2]+n[1]*Xc[1]/Xc[2]+n[2]);
    X[0]=d*Xc[0]/Xc[2];
    X[1]=d*Xc[1]/Xc[2];
    X[2]=d;

    if((fabs(nn[0])+fabs(nn[1]))<1e-13)
    {
        t[0]=-(R[0]*X[0]+R[3]*X[1]+R[6]*X[2]);
        t[1]=-(R[1]*X[0]+R[4]*X[1]+R[7]*X[2]);
        t[2]=dist-(R[2]*X[0]+R[5]*X[1]+R[8]*X[2]);
        
    }
    else
    {
        t[0]=-(R[0]*X[0]+R[1]*X[1]+R[2]*X[2]);
        t[1]=-(R[3]*X[0]+R[4]*X[1]+R[5]*X[2]);
        t[2]=dist-(R[6]*X[0]+R[7]*X[1]+R[8]*X[2]);
        q[1]=-q[1];
        q[2]=-q[2];
    }
    
}

